#include "Components/ComponentBody.h"
#include "Components/ComponentLocomotion.h"
#include "Components/ComponentCreature.h"
#include "Components/ComponentHealth.h"

#include "Managers/BlocksManager.h"
#include "Managers/EntityManager.h"
#include "Managers/GameManager.h"

#include "Subsystems/SubsystemTerrain.h"
#include "Subsystems/SubsystemBodies.h"
#include "Subsystems/SubsystemTimeOfDay.h"

#include "Terrain/ScSimplexNoise.h"

#include "Subsystems/BlockBehaviors/SubsystemBlockBehaviors.h"
#include "Subsystems/BlockBehaviors/SubsystemWaterBlockBehavior.h"

#include "Network/PackageManager.h"
#include "Network/Packages/SubsystemBodyPackage.h"

#include "GameSingleton.h"
#include "GameRandom.hpp"
#include "GameRegistry.hpp"
#include "ServerConfigure.h"
#include "GameThreadPool.hpp"
#include "Log.h"

#include "Expand/EventManager.h"
#include <cfloat>

namespace PocketSurvival
{
    const std::vector<Vector3> ComponentBody::freeSpaceOffsets = 
    {
        Vector3(-0.5,0,0),
        Vector3(0.5,0,0),
        Vector3(0,-0.5,0),
        Vector3(0,0.5,0),
        Vector3(0,0,-0.5),
        Vector3(0,0,0.5),
        Vector3(-0.5,0,0.5),
        Vector3(-0.5,0.5,0),
        Vector3(0,-0.5,-0.5),
        Vector3(0,0.5,-0.5),
        Vector3(0,0.5,0.5),
        Vector3(0.5,-0.5,0),
        Vector3(0.5,0,-0.5),
        Vector3(0.5,0,0.5),
        Vector3(0.5,0.5,0),
        Vector3(-0.5,0,-0.5),
        Vector3(-0.5,-0.5,0),
        Vector3(0,-0.5,0.5),
        Vector3(-0.5,0.5,0.5),
        Vector3(-0.5,0.5,-0.5),
        Vector3(0.5,-0.5,-0.5),
        Vector3(0.5,0.5,-0.5),
        Vector3(0.5,0.5,0.5),
        Vector3(-0.5,-0.5,0.5),
        Vector3(0.5,-0.5,0.5),
        Vector3(-0.5,-0.5,-0.5),
        Vector3(0,-1.0,0),
        Vector3(-1.0,0,0),
        Vector3(1.0,0,0),
        Vector3(0,1.0,0),
        Vector3(0,0,1.0),
        Vector3(0,0,-1.0),
        Vector3(0,0.5,-1.0),
        Vector3(-0.5,1.0,0),
        Vector3(0,-1.0,-0.5),
        Vector3(0,-0.5,1.0),
        Vector3(0,-1.0,0.5),
        Vector3(0.5,-1.0,0),
        Vector3(0,1.0,0.5),
        Vector3(0,-0.5,-1.0),
        Vector3(-1.0,0,-0.5),
        Vector3(0,1.0,-0.5),
        Vector3(0.5,0,-1.0),
        Vector3(1.0,0.5,0),
        Vector3(0.5,1.0,0),
        Vector3(1.0,0,0.5),
        Vector3(1.0,0,-0.5),
        Vector3(1.0,-0.5,0),
        Vector3(-0.5,-1.0,0),
        Vector3(0,0.5,1.0),
        Vector3(-1.0,0.5,0),
        Vector3(-1.0,-0.5,0),
        Vector3(-0.5,0,1.0),
        Vector3(0.5,0,1.0),
        Vector3(-1.0,0,0.5),
        Vector3(-0.5,0,-1.0),
        Vector3(0.5,0.5,1.0),
        Vector3(0.5,0.5,-1.0),
        Vector3(0.5,1.0,-0.5),
        Vector3(0.5,-1.0,0.5),
        Vector3(0.5,1.0,0.5),
        Vector3(0.5,-1.0,-0.5),
        Vector3(0.5,-0.5,-1.0),
        Vector3(1.0,0.5,0.5),
        Vector3(1.0,-0.5,-0.5),
        Vector3(1.0,-0.5,0.5),
        Vector3(0.5,-0.5,1.0),
        Vector3(-1.0,-0.5,-0.5),
        Vector3(-1.0,-0.5,0.5),
        Vector3(-0.5,-1.0,-0.5),
        Vector3(-0.5,-1.0,0.5),
        Vector3(-0.5,-0.5,-1.0),
        Vector3(-0.5,-0.5,1.0),
        Vector3(-1.0,0.5,0.5),
        Vector3(-0.5,0.5,-1.0),
        Vector3(-0.5,0.5,1.0),
        Vector3(-0.5,1.0,-0.5),
        Vector3(-0.5,1.0,0.5),
        Vector3(-1.0,0.5,-0.5),
        Vector3(1.0,0.5,-0.5),
        Vector3(-1.0,0,1.0),
        Vector3(0,1.0,-1.0),
        Vector3(-1.0,0,-1.0),
        Vector3(0,1.0,1.0),
        Vector3(1.0,0,-1.0),
        Vector3(-1.0,1.0,0),
        Vector3(-1.0,-1.0,0),
        Vector3(1.0,-1.0,0),
        Vector3(1.0,1.0,0),
        Vector3(1.0,0,1.0),
        Vector3(0,-1.0,1.0),
        Vector3(0,-1.0,-1.0),
        Vector3(-1.0,1.0,-0.5),
        Vector3(1.0,-1.0,0.5),
        Vector3(1.0,0.5,-1.0),
        Vector3(1.0,-0.5,1.0),
        Vector3(-1.0,1.0,0.5),
        Vector3(-0.5,-1.0,-1.0),
        Vector3(1.0,-0.5,-1.0),
        Vector3(1.0,1.0,-0.5),
        Vector3(-0.5,1.0,1.0),
        Vector3(-1.0,-0.5,1.0),
        Vector3(1.0,0.5,1.0),
        Vector3(0.5,1.0,1.0),
        Vector3(0.5,-1.0,1.0),
        Vector3(-0.5,-1.0,1.0),
        Vector3(1.0,1.0,0.5),
        Vector3(0.5,1.0,-1.0),
        Vector3(-1.0,-1.0,-0.5),
        Vector3(-1.0,0.5,-1.0),
        Vector3(0.5,-1.0,-1.0),
        Vector3(-1.0,-1.0,0.5),
        Vector3(-0.5,1.0,-1.0),
        Vector3(-1.0,-0.5,-1.0),
        Vector3(1.0,-1.0,-0.5),
        Vector3(-1.0,0.5,1.0),
        Vector3(1.0,1.0,-1.0),
        Vector3(-1.0,-1.0,-1.0),
        Vector3(1.0,-1.0,-1.0),
        Vector3(-1.0,1.0,1.0),
        Vector3(-1.0,1.0,-1.0),
        Vector3(-1.0,-1.0,1.0),
        Vector3(1.0,-1.0,1.0),
        Vector3(1.0,1.0,1.0)
    };



    static float calculateBoxBoxOverlap(const BoundingBox &box1, const BoundingBox &box2, int32_t axis)
    {
        if (box1.Max.posX <= box2.Min.posX ||
            box1.Min.posX >= box2.Max.posX ||
            box1.Max.posY <= box2.Min.posY ||
            box1.Min.posY >= box2.Max.posY ||
            box1.Max.posZ <= box2.Min.posZ ||
            box1.Min.posZ >= box2.Max.posZ)
        {
            return 0.0f;
        }
        switch (axis)
        {
        case 0:
        {
            float num13 = box1.Min.posX + box1.Max.posX;
            float num14 = box2.Min.posX + box2.Max.posX;
            float num15 = box1.Max.posX - box1.Min.posX;
            float num16 = box2.Max.posX - box2.Min.posX;
            float num17 = num14 - num13;
            float num18 = num15 + num16;
            return 0.5f * ((num17 > 0.0f) ? (num17 - num18) : (num17 + num18));
        }
        case 1:
        {
            float num7 = box1.Min.posY + box1.Max.posY;
            float num8 = box2.Min.posY + box2.Max.posY;
            float num9 = box1.Max.posY - box1.Min.posY;
            float num10 = box2.Max.posY - box2.Min.posY;
            float num11 = num8 - num7;
            float num12 = num9 + num10;
            return 0.5f * ((num11 > 0.0f) ? (num11 - num12) : (num11 + num12));
        }
        default:
        {
            float num = box1.Min.posZ + box1.Max.posZ;
            float num2 = box2.Min.posZ + box2.Max.posZ;
            float num3 = box1.Max.posZ - box1.Min.posZ;
            float num4 = box2.Max.posZ - box2.Min.posZ;
            float num5 = num2 - num;
            float num6 = num3 + num4;
            return 0.5f * ((num5 > 0.0f) ? (num5 - num6) : (num5 + num6));
        }
        }
    }

    static inline float getVectorComponent(const Vector3 &v, int32_t axis)
    {
        switch (axis)
        {
        case 0:
            return v.posX;
        case 1:
            return v.posY;
        default:
            return v.posZ;
        }
    }

    static inline void inelasticCollision(float v1, float v2, float m1, float m2, float cr, float &result1, float &result2)
    {
        float num = 1.0f / (m1 + m2);
        result1 = (cr * m2 * (v2 - v1) + m1 * v1 + m2 * v2) * num;
        result2 = (cr * m1 * (v1 - v2) + m1 * v1 + m2 * v2) * num;
    }

    static const CollisionBox* getIsCollidingBox(const BoundingBox &box, std::vector<CollisionBox> &collisionBoxes1, std::vector<CollisionBox> &collisionBoxes2, std::vector<CollisionBox> &collisionBoxes3)
    {
        for(const CollisionBox& collisionBox : collisionBoxes1)
        {
            if (box.intersection(collisionBox.box))
            {
                return &collisionBox;
            }
        }
        for(const CollisionBox& collisionBox : collisionBoxes2)
        {
            if (box.intersection(collisionBox.box))
            {
                return &collisionBox;
            }
        }
        for(const CollisionBox& collisionBox : collisionBoxes3)
        {
            if (box.intersection(collisionBox.box))
            {
                return &collisionBox;
            }
        }
        return nullptr;
    }

    void ComponentBody::getPosition(Vector3 &outPosition)
    {
        m_positionMutex.lock();
        outPosition = m_position;
        m_positionMutex.unlock();
    }
    void ComponentBody::setPosition(const Vector3 &position)
    {
        std::lock_guard<std::recursive_mutex> lk(m_positionMutex);
        m_position = position;
        m_needUpdateMatrix = true;
        m_positionChange = true;
    }

    void ComponentBody::getVelocity(Vector3 &outVelocity)
    {
        m_velocityMutex.lock();
        outVelocity = m_velocity;
        m_velocityMutex.unlock();
    }
    void ComponentBody::setVelocity(const Vector3 &velocity)
    {
        std::lock_guard<std::recursive_mutex> lk(m_velocityMutex);
        if (velocity.lengthSquared() > 625.0f)
        {
            m_velocity = 25.0f * Vector3::Normalize(velocity);
        }
        else
        {
            m_velocity = velocity;
        }
        m_velocityChange = true;
    }

    void ComponentBody::getRotation(Quaternion &outRotation)
    {
        m_rotationMutex.lock();
        outRotation = m_rotation;
        m_rotationMutex.unlock();
    }

    void ComponentBody::setRotation(const Quaternion &rotation)
    {
        std::lock_guard<std::recursive_mutex> lk(m_rotationMutex);
        m_rotation = Quaternion::Normalize(rotation);
        m_needUpdateMatrix = true;
        m_rotationChange = true;
    }

    void ComponentBody::setSneaking(bool value)
    {
        m_sneakingChange = true;
        isSneaking = value;
    }



    void ComponentBody::addCollidedWithBodyEvent(std::function<void(ComponentBody*)> event)
    {
        m_collidedWithBodyEvents += event;
    }

    void ComponentBody::applyImpulse(const Vector3 &impulse)
    {
        GameSingleton::packageManager->addSendPackageToQueue(std::make_shared<SubsystemBodyPackage>(this, impulse));
        applyImpulseNet(impulse);
    }
    void ComponentBody::applyImpulseNet(const Vector3 &impulse)
    {
        totalImpulse += impulse;
    }
    void ComponentBody::applyDirectMove(const Vector3 &move)
    {
        directMove += move;
    }

    // 获取实体的体型框，一般用于体型碰撞计算
    BoundingBox ComponentBody::getBoundingBox()
    {
        std::lock_guard<std::recursive_mutex> lk(m_positionMutex);
        return BoundingBox(m_position - Vector3(boxSize.posX / 2.0f, 0.0f, boxSize.posZ / 2.0f), m_position + Vector3(boxSize.posX / 2.0f, boxSize.posY, boxSize.posZ / 2.0f));
    }

    // 获取标准的体型大小，蹲下和站立的体型大小是不一样的
    Vector3 ComponentBody::getStanceBoxSize()
    {
        return Vector3(boxSize.posX, (isSneaking ? 0.4f : 1.0f) * boxSize.posY, boxSize.posZ);
    }


    bool ComponentBody::getChangedPosition(Vector3 &pos)
    {
        if (m_positionChange.exchange(false) == true)
        {
            m_positionMutex.lock();
            pos = m_position;
            m_positionMutex.unlock();
            return true;
        }
        return false;
    }
    bool ComponentBody::getChangedRotation(Quaternion &rota)
    {
        if (m_rotationChange.exchange(false) == true)
        {
            m_rotationMutex.lock();
            rota = m_rotation;
            m_rotationMutex.unlock();
            return true;
        }
        return false;
    }
    bool ComponentBody::getChangedVelocity(Vector3 &vel)
    {
        if (m_velocityChange.exchange(false) == true)
        {
            m_velocityMutex.lock();
            vel = m_velocity;
            m_velocityMutex.unlock();
            return true;
        }
        return false;
    }
    bool ComponentBody::getSneaking(bool &value)
    {
        if (m_sneakingChange.exchange(false) == true)
        {
            value = isSneaking;
            return true;
        }
        return false;
    }


    void ComponentBody::update(float dt)
    {
        SubsystemTerrain *subsystemTerrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();
        SubsystemTimeOfDay *timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
        SubsystemWaterBlockBehavior *waterBlockBehavior = GameSingleton::singletonPtr->getSingleton<SubsystemWaterBlockBehavior>();

        Vector3 position;
        Vector3 velocity;
        Quaternion rotation;
        getPosition(position);
        getVelocity(velocity);
        getRotation(rotation);

        if (velocity.lengthSquared() < 0.001f || velocity.lengthSquared() >= 9999.0f)
        {
            velocity = FastVector3::Zero;
        }
        velocity += totalImpulse;
        totalImpulse = FastVector3::Zero;

        // 实时位置检测
        GameSingleton::eventManager->creatureEvent->realTimePosition(this);

        TerrainChunk *chunkAtCell = subsystemTerrain->getTerrainChunk(Terrain::ToCell(position.posX) >> 4, Terrain::ToCell(position.posZ) >> 4);
        if (chunkAtCell == nullptr || chunkAtCell->State <= TerrainChunkState::InvalidContents4)
        {
            velocity = FastVector3::Zero;
            setVelocity(velocity);
            return;
        }


        // 挤压伤害暂时不考虑实体
        bodiesCollisionBoxes.clear();
        // findBodiesCollisionBoxes(body->position, body->bodiesCollisionBoxes);

        movingBlocksCollisionBoxes.clear();
        findMovingBlocksCollisionBoxes(position, movingBlocksCollisionBoxes);

        // 卡住地形中受伤
        if (moveToFreeSpace(0.1f) == false)
        {
            if (componentCreature != nullptr)
            {
                if (crushInjureTime >= 1.0f)
                {
                    componentCreature->componentHealth->injure(0.15f, entt::null, true, "挤压而死");
                    crushInjureTime = 0.0f;
                }
                crushInjureTime += dt;
            }
            else
            {
                Log::Warn("出现位置的实体body错误");
            }
            return;
        }
        else
        {
            crushInjureTime = 0.0f;
        }

        // 玩家位置由客户端来确定，这里就不进行矫正了
        if (componentPlayer != nullptr)
        {
            return;
        }

        // 是否有重力
        if (isGravityEnabled)
        {
            velocity.posY -= 10.0f * dt;
            if (immersionFactor > 0.0f)
            {
                float num = immersionFactor * (1.0f + 0.03f * MathUtils::Sin<float>((float)MathUtils::Remainder(2.0 * timeOfDay->getGameTime(), 6.2831854820251465)));
                velocity.posY += 10.0f * (1.0f / density * num) * dt;
            }
        }



        float num2 = MathUtils::Saturate(airDrag.posX * dt);
        float num3 = MathUtils::Saturate(airDrag.posY * dt);
        velocity.posX *= 1.0f - num2;
        velocity.posY *= 1.0f - num3;
        velocity.posZ *= 1.0f - num2;

        if(velocity.lengthSquared() < 0.0002f)
        {
            velocity = FastVector3::Zero;
        }
        setVelocity(velocity);



        if (immersionFactor > 0.0f && immersionFluidBlock != nullptr)
        {
            std::shared_ptr<Vector2> vector = waterBlockBehavior->calculateFlowSpeed(Terrain::ToCell(position.posX), Terrain::ToCell(position.posY), Terrain::ToCell(position.posZ));
            Vector3 vector2 = (vector != nullptr ? Vector3(vector->posX, 0.0f, vector->posY) : FastVector3::Zero);
            float num4 = 1.0f;
            if (immersionFluidBlock->FrictionFactor != 1.0f)
            {
                num4 = ((ScSimplexNoise::Noise((float)MathUtils::Remainder(6.0 * dt + (double)(GameSingleton::gameRandom->getInt() % 1000), 1000.0)) > 0.5f) ? immersionFluidBlock->FrictionFactor : 1.0f);
            }
            float f = MathUtils::Saturate(waterDrag.posX * num4 * immersionFactor * dt);
            float f2 = MathUtils::Saturate(waterDrag.posY * num4 * dt);

            velocity.posX = MathUtils::Lerp(velocity.posX, vector2.posX, f);
            velocity.posY = MathUtils::Lerp(velocity.posY, vector2.posY, f2);
            velocity.posZ = MathUtils::Lerp(velocity.posZ, vector2.posZ, f);
            setVelocity(velocity);

            if (vector != nullptr && standingOnValue == 0)
            {
                Quaternion quration;
                if (waterTurnSpeed > 0.0f)
                {
                    float s = MathUtils::Saturate(MathUtils::Lerp(1.0f, 0.0f, velocity.length()));
                    Vector2 vector3 = Vector2::Normalize(*vector) * s;
                    Quaternion::CreateFromAxisAngle(FastVector3::UnitY, waterTurnSpeed * (-1.0f * vector3.posX + 0.71f * vector3.posY) * dt, quration);
                    rotation *= quration;
                    setRotation(Quaternion::Normalize(rotation));
                }
                if (waterSwayAngle > 0.0f)
                {
                    Quaternion::CreateFromAxisAngle(FastVector3::UnitX, waterSwayAngle * (float)MathUtils::Sin((double)(200.0f / mass) * timeOfDay->getGameTime()), quration);
                    rotation *= quration;
                    setRotation(Quaternion::Normalize(rotation));
                }
            }
        }

        standingOnValue = 0;
        standingOnBody = nullptr;
        standingOnVelocity = FastVector3::Zero;
        float num5 = velocity.length();

        const Vector3 constVelocity = velocity;

        if (num5 > 0.0f)
        {
            Vector3 stanceBoxSize = getStanceBoxSize();
        	float x = 0.45f * MathUtils::Min<float>(stanceBoxSize.posX, stanceBoxSize.posY, stanceBoxSize.posZ) / num5;
        	float num6 = dt;
        	while (num6 > 0.0f)
        	{
        		float num7 = MathUtils::Min<float>(num6, x);
                Vector3 theVelocity = velocity * num7 + directMove;

        		moveWithCollision(num7, theVelocity);
        		directMove = FastVector3::Zero;
        		num6 -= num7;
        	}
        }

        collisionVelocityChange = velocity - constVelocity;
        if (standingOnValue != 0)
        {
        	Vector3::Lerp(constVelocity, standingOnVelocity, 6.0f * dt, velocity);
        }
        updateImmersionData();
    }

    /***********************************************************************************************************
     * @brief 计算流动方块的浸没深度，以及获取相应的流动方块
     ***********************************************************************************************************/
    void ComponentBody::updateImmersionData()
    {
        Vector3 pos;
        getPosition(pos);

        int32_t x = Terrain::ToCell(pos.posX);
        int32_t y = Terrain::ToCell(pos.posY + 0.01f);
        int32_t z = Terrain::ToCell(pos.posZ);
        std::shared_ptr<float> surfaceHeight = GameSingleton::singletonPtr->getSingleton<SubsystemWaterBlockBehavior>()->getSurfaceHeight(x, y, z);
        if (surfaceHeight != nullptr)
        {
            int32_t cellValue = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>()->getCellValue(x, y, z);
            immersionDepth = MathUtils::Max<float>(*surfaceHeight - pos.posY, 0.0f);
            immersionFactor = MathUtils::Saturate(MathUtils::Pow(immersionDepth / getStanceBoxSize().posY, 0.7f));
            immersionFluidBlock = GameSingleton::blocksManager->blocks[Terrain::ExtractContents(cellValue)]->asBlock<FluidBlock *>();
        }
        else
        {
            immersionDepth = 0.0f;
            immersionFactor = 0.0f;
            immersionFluidBlock = nullptr;
        }
    }

    /***********************************************************************************************************
     * @brief 被方块或其它实体卡住的时候，移动到空旷的地方
     * @param maxMoveDistance 最大的移动距离
    ***********************************************************************************************************/
    bool ComponentBody::moveToFreeSpace(float maxMoveDistance)
    {
        Vector3 position;
        getPosition(position);

        Vector3 stanceBoxSize = getStanceBoxSize();
        Vector3 v1(stanceBoxSize.posX / 2.0f, 0.0f, stanceBoxSize.posZ / 2.0f);
        Vector3 v2(stanceBoxSize.posX / 2.0f, stanceBoxSize.posY, stanceBoxSize.posZ / 2.0f);

        BoundingBox bodyBox(position - v1, position + v2);
        bodyBox.Min += Vector3(0.01f, maxSmoothRiseHeight + 0.01f, 0.01f);
        bodyBox.Max -= 0.01f;

        collisionBoxes.clear();
        findTerrainCollisionBoxes(bodyBox, collisionBoxes);
        const CollisionBox *collidingBox = getIsCollidingBox(bodyBox, collisionBoxes, movingBlocksCollisionBoxes, bodiesCollisionBoxes);

        if(collidingBox == nullptr)
        {
            return true;
        }
        else if(componentPlayer != nullptr)
        {
            return false;
        }
        else
        {
            const BoundingBox &targetBox = collidingBox->box;

            Vector3 thePos;
            for (const auto& item : freeSpaceOffsets) 
            {
                thePos = position + item;
                bodyBox.Min = thePos - v1 + Vector3(0.01f, maxSmoothRiseHeight + 0.01f, 0.01f);
                bodyBox.Max = thePos + v2 - 0.01f;

                const CollisionBox *collidingBox = getIsCollidingBox(bodyBox, collisionBoxes, movingBlocksCollisionBoxes, bodiesCollisionBoxes);
                if(collidingBox == nullptr)
                {
                    setPosition(thePos);
                    return true;
                }
            }
        }
        return false;
    }

    void ComponentBody::moveWithCollision(float dt, Vector3 &move)
    {
        Vector3 pos;
        getPosition(pos);

        bool isSmoothRising = maxSmoothRiseHeight > 0.0f && handleSmoothRise(move, pos, dt);
        handleAxisCollision(1, move.posY, pos, isSmoothRising);
        handleAxisCollision(0, move.posX, pos, isSmoothRising);
        handleAxisCollision(2, move.posZ, pos, isSmoothRising);
        setPosition(pos);
    }

    bool ComponentBody::handleSmoothRise(Vector3 &move, const Vector3 &position, float dt)
    {
        Vector3 boxSize = getStanceBoxSize();
        BoundingBox box = BoundingBox(position - Vector3(boxSize.posX / 2.0f, 0.0f, boxSize.posZ / 2.0f), position + Vector3(boxSize.posX / 2.0f, boxSize.posY, boxSize.posZ / 2.0f));
        box.Min += Vector3(0.04f, 0.0f, 0.04f);
        box.Max -= Vector3(0.04f, 0.0f, 0.04f);
        collisionBoxes.clear();
        findTerrainCollisionBoxes(box, collisionBoxes);
        for (const CollisionBox &item : movingBlocksCollisionBoxes)
        {
            collisionBoxes.push_back(item);
        }
        CollisionBox pushingCollisionBox;
        float num = MathUtils::Max<float>(calculatePushBack(box, 1, collisionBoxes, pushingCollisionBox), 0.0f);
        if (GameSingleton::blocksManager->blocks[Terrain::ExtractContents(pushingCollisionBox.blockValue)]->NoSmoothRise == false && num > 0.04f)
        {
            float x = MathUtils::Min<float>(4.5f * dt, num);
            move.posY = MathUtils::Max<float>(move.posY, x);

            Vector3 velocity;
            getVelocity(velocity);
            velocity.posY = MathUtils::Max<float>(velocity.posY, 0.0f);
            setVelocity(velocity);

            standingOnValue = pushingCollisionBox.blockValue;
            standingOnBody = pushingCollisionBox.componentBody;
            return true;
        }
        return false;
    }

    void ComponentBody::handleAxisCollision(int32_t axis, float move, Vector3 &position, bool isSmoothRising)
    {
        Vector3 velocity;
        getVelocity(velocity);

        Vector3 boxSize = getStanceBoxSize();
        collisionBoxes.clear();
        if (isSneaking && axis != 1)
        {
            findSneakCollisionBoxes(position, Vector2(boxSize.posX - 0.08f, boxSize.posZ - 0.08f), collisionBoxes);
        }
        Vector3 v;
        switch (axis)
        {
        case 0:
            position.posX += move;
            v = Vector3(0.0f, 0.04f, 0.04f);
            break;
        case 1:
            position.posY += move;
            v = Vector3(0.04f, 0.0f, 0.04f);
            break;
        default:
            position.posZ += move;
            v = Vector3(0.04f, 0.04f, 0.0f);
            break;
        }

        BoundingBox boundingBox
        {
            position - Vector3(boxSize.posX / 2.0f, 0.0f, boxSize.posZ / 2.0f) + v, 
            position + Vector3(boxSize.posX / 2.0f, boxSize.posY, boxSize.posZ / 2.0f) - v
        };

        findTerrainCollisionBoxes(boundingBox, collisionBoxes);
        for (const CollisionBox &item : movingBlocksCollisionBoxes)
        {
            collisionBoxes.push_back(item);
        }
        float num;
        CollisionBox pushingCollisionBox;
        if ((axis != 1) || isSmoothRising)
        {
            BoundingBox smoothRiseBox = boundingBox;
            smoothRiseBox.Min.posY += maxSmoothRiseHeight;
            num = calculateSmoothRisePushBack(boundingBox, smoothRiseBox, axis, collisionBoxes, pushingCollisionBox);
        }
        else
        {
            num = calculatePushBack(boundingBox, axis, collisionBoxes, pushingCollisionBox);
        }
        BoundingBox box = BoundingBox(position - Vector3(boxSize.posX / 2.0f, 0.0f, boxSize.posZ / 2.0f) + v, position + Vector3(boxSize.posX / 2.0f, boxSize.posY, boxSize.posZ / 2.0f) - v);
        CollisionBox pushingCollisionBox2;
        // 客户端num2为0
        float num2 = calculatePushBack(box, axis, bodiesCollisionBoxes, pushingCollisionBox2);
        if (MathUtils::Abs<float>(num) > MathUtils::Abs<float>(num2))
        {
            if (num == 0.0f)
            {
                return;
            }



            int32_t num3 = Terrain::ExtractContents(pushingCollisionBox.blockValue);
            if (GameSingleton::blocksManager->blocks[num3]->HasCollisionBehavior)
            {
                std::vector<SubsystemBlockBehavior *> &blockBehaviors = GameSingleton::singletonPtr->getSingleton<SubsystemBlockBehaviors>()->getBlockBehaviors(num3);
                for (SubsystemBlockBehavior *behavior : blockBehaviors)
                {
                    Vector3 vector = (pushingCollisionBox.box.Min + pushingCollisionBox.box.Max) / 2.0f;
                    CellFace cellFace = CellFace::FromAxisAndDirection(Terrain::ToCell(vector.posX), Terrain::ToCell(vector.posY), Terrain::ToCell(vector.posZ), axis, 0.0f - getVectorComponent(velocity, axis));
                    behavior->onCollide(cellFace, getVectorComponent(velocity, axis), this);
                }
            }

            switch (axis)
            {
            case 0:
                position.posX += num;
                velocity.posX = pushingCollisionBox.blockVelocity.posX;
                break;
            case 1:
                position.posY += num;
                velocity.posY = pushingCollisionBox.blockVelocity.posY;
                if (move < 0.0f)
                {
                    standingOnValue = pushingCollisionBox.blockValue;
                    standingOnBody = pushingCollisionBox.componentBody;
                    standingOnVelocity = pushingCollisionBox.blockVelocity;
                }
                break;
            default:
                position.posZ += num;
                velocity.posZ = pushingCollisionBox.blockVelocity.posZ;
                break;
            }
            setPosition(position);
            setVelocity(velocity);
        }
        else
        {
            if (num2 == 0.0f)
            {
                return;
            }

            // 碰撞对方，使对方发生移动
            ComponentBody *componentBody = pushingCollisionBox2.componentBody;
            Vector3 targetVelocity;
            componentBody->getVelocity(targetVelocity);

            switch (axis)
            {
            case 0:
                inelasticCollision(velocity.posX, targetVelocity.posX, mass, componentBody->mass, 0.5f, velocity.posX, targetVelocity.posX);
                position.posX += num2;
                break;
            case 1:
                inelasticCollision(velocity.posY, targetVelocity.posY, mass, componentBody->mass, 0.5f, velocity.posY, targetVelocity.posY);
                position.posY += num2;
                if (move < 0.0f)
                {
                    standingOnValue = pushingCollisionBox2.blockValue;
                    standingOnBody = pushingCollisionBox2.componentBody;
                    standingOnVelocity = Vector3(targetVelocity.posX, 0.0f, targetVelocity.posZ);
                }
                break;
            default:
                inelasticCollision(velocity.posZ, targetVelocity.posZ, mass, componentBody->mass, 0.5f, velocity.posZ, targetVelocity.posZ);
                position.posZ += num2;
                break;
            }

            m_collidedWithBodyEvents(componentBody);
            componentBody->m_collidedWithBodyEvents(componentBody);
        }
    }

    void ComponentBody::findBodiesCollisionBoxes(const Vector3 &position, std::vector<CollisionBox> &result)
    {
        componentBodies.clear();
        GameSingleton::singletonPtr->getSingleton<SubsystemBodies>()->findBodiesAroundPoint(Vector2(position.posX, position.posZ), 4.0f, componentBodies);
        for (ComponentBody *componentBody : componentBodies)
        {
            // 动物骑乘 未完成...
            if (componentBody != this)
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    componentBody,
                    componentBody->getBoundingBox());
            }
        }
    }

    void ComponentBody::findMovingBlocksCollisionBoxes(const Vector3 &position, std::vector<CollisionBox> &result)
    {
        BoundingBox boundingBox{
            position - Vector3(boxSize.posX / 2.0f, 0.0f, boxSize.posZ / 2.0f),
            position + Vector3(boxSize.posX / 2.0f, boxSize.posY, boxSize.posZ / 2.0f)};
        boundingBox.Min -= 1.0f;
        boundingBox.Max += 1.0f;

        // 移动方块 未完成...
        /*

        m_movingBlockSets.Clear();
        m_subsystemMovingBlocks.FindMovingBlocks(boundingBox, extendToFillCells: false, m_movingBlockSets);
        for (int32_t i = 0; i < m_movingBlockSets.Count; i++)
        {
            IMovingBlockSet movingBlockSet = m_movingBlockSets.Array[i];
            for (int32_t j = 0; j < movingBlockSet.Blocks.Count; j++)
            {
                MovingBlock movingBlock = movingBlockSet.Blocks[j];
                int32_t num = Terrain.ExtractContents(movingBlock.Value);
                Block block = BlocksManager.Blocks[num];
                if (block.IsCollidable)
                {
                    BoundingBox[] customCollisionBoxes = block.GetCustomCollisionBoxes(m_subsystemTerrain, movingBlock.Value);
                    Vector3 v = new Vector3(movingBlock.Offset) + movingBlockSet.Position;
                    for (int32_t k = 0; k < customCollisionBoxes.Length; k++)
                    {
                        result.Add(new CollisionBox
                        {
                            Box = new BoundingBox(v + customCollisionBoxes[k].Min, v + customCollisionBoxes[k].Max),
                            BlockValue = movingBlock.Value,
                            BlockVelocity = movingBlockSet.CurrentVelocity
                        });
                    }
                }
            }
        }

        */
    }

    void ComponentBody::findTerrainCollisionBoxes(const BoundingBox &box, std::vector<CollisionBox> &result)
    {
        Point3 point = Terrain::ToCell(box.Min) - 1;
        Point3 point2 = Terrain::ToCell(box.Max) + 1;
        point.posY = MathUtils::Max<int32_t>(point.posY, 0);
        point2.posY = MathUtils::Min<int32_t>(point2.posY, 255);
        if (point.posY > point2.posY)
        {
            return;
        }
        SubsystemTerrain *subsystemTerrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();
        for (int32_t i = point.posX; i <= point2.posX; i++)
        {
            for (int32_t j = point.posZ; j <= point2.posZ; j++)
            {
                TerrainChunk *chunkAtCell = subsystemTerrain->getTerrainChunk(i >> 4, j >> 4);
                if (chunkAtCell == nullptr)
                {
                    continue;
                }
                int32_t num = TerrainChunk::CalculateCellIndex(i & 0xF, point.posY, j & 0xF);
                int32_t num2 = point.posY;
                while (num2 <= point2.posY)
                {
                    int32_t cellValueFast = chunkAtCell->getCellValueFast(num);
                    int32_t num3 = Terrain::ExtractContents(cellValueFast);
                    if (num3 != 0)
                    {
                        Block *block = GameSingleton::blocksManager->blocks[num3];
                        if (block->IsCollidable)
                        {
                            const std::vector<BoundingBox> &customCollisionBoxes = block->getCustomCollisionBoxes(cellValueFast);
                            Vector3 v(i, num2, j);
                            for (int32_t k = 0; k < customCollisionBoxes.size(); k++)
                            {
                                result.emplace_back(
                                    cellValueFast,
                                    FastVector3::Zero,
                                    nullptr,
                                    BoundingBox(v + customCollisionBoxes[k].Min, v + customCollisionBoxes[k].Max));
                            }
                        }
                    }
                    num2++;
                    num++;
                }
            }
        }
    }

    void ComponentBody::findSneakCollisionBoxes(const Vector3 &position, const Vector2 &overhang, std::vector<CollisionBox> &result)
    {
        // BlocksManager *blocksManager = GameSingleton::singletonPtr->getSingleton<BlocksManager>();
        SubsystemTerrain *subsystemTerrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();

        int32_t num = Terrain::ToCell(position.posX);
        int32_t num2 = Terrain::ToCell(position.posY);
        int32_t num3 = Terrain::ToCell(position.posZ);
        if (GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num, num2 - 1, num3)]->IsCollidable)
        {
            return;
        }
        bool num4 = position.posX < (float)num + 0.5f;
        bool flag = position.posZ < (float)num3 + 0.5f;
        if (num4)
        {
            if (flag)
            {
                bool isCollidable  = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num, num2 - 1, num3 - 1)]->IsCollidable;
                bool isCollidable2 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num - 1, num2 - 1, num3)]->IsCollidable;
                bool isCollidable3 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num - 1, num2 - 1, num3 - 1)]->IsCollidable;
                if ((isCollidable && !isCollidable2) || ((!isCollidable && !isCollidable2) & isCollidable3))
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox(Vector3(num, num2, (float)num3 + overhang.posY), Vector3(num + 1, num2 + 1, num3 + 1)));
                }
                if ((!isCollidable && isCollidable2) || ((!isCollidable && !isCollidable2) & isCollidable3))
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox(Vector3((float)num + overhang.posX, num2, num3), Vector3(num + 1, num2 + 1, num3 + 1)));
                }
                if (isCollidable && isCollidable2)
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox(Vector3((float)num + overhang.posX, num2, (float)num3 + overhang.posY), Vector3(num + 1, num2 + 1, num3 + 1)));
                }
            }
            else
            {
                bool isCollidable4 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num, num2 - 1, num3 + 1)]->IsCollidable;
                bool isCollidable5 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num - 1, num2 - 1, num3)]->IsCollidable;
                bool isCollidable6 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num - 1, num2 - 1, num3 + 1)]->IsCollidable;
                if ((isCollidable4 && !isCollidable5) || ((!isCollidable4 && !isCollidable5) & isCollidable6))
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox( Vector3(num, num2, num3), Vector3(num + 1, num2 + 1, (float)(num3 + 1) - overhang.posY)));
                }
                if ((!isCollidable4 && isCollidable5) || ((!isCollidable4 && !isCollidable5) & isCollidable6))
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox( Vector3((float)num + overhang.posX, num2, num3), Vector3(num + 1, num2 + 1, num3 + 1)));
                }
                if (isCollidable4 && isCollidable5)
                {
                    result.emplace_back(
                        0,
                        FastVector3::Zero,
                        nullptr,
                        BoundingBox( Vector3((float)num + overhang.posX, num2, num3), Vector3(num + 1, num2 + 1, (float)(num3 + 1) - overhang.posY)));
                }
            }
        }
        else if (flag)
        {
            bool isCollidable7 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num, num2 - 1, num3 - 1)]->IsCollidable;
            bool isCollidable8 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num + 1, num2 - 1, num3)]->IsCollidable;
            bool isCollidable9 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num + 1, num2 - 1, num3 - 1)]->IsCollidable;
            if ((isCollidable7 && !isCollidable8) || ((!isCollidable7 && !isCollidable8) & isCollidable9))
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, (float)num3 + overhang.posY), Vector3(num + 1, num2 + 1, num3 + 1)));
            }
            if ((!isCollidable7 && isCollidable8) || ((!isCollidable7 && !isCollidable8) & isCollidable9))
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, num3), Vector3((float)(num + 1) - overhang.posX, num2 + 1, num3 + 1)));
            }
            if (isCollidable7 && isCollidable8)
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, (float)num3 + overhang.posY), Vector3((float)(num + 1) - overhang.posX, num2 + 1, num3 + 1)));
            }
        }
        else
        {
            bool isCollidable10 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num, num2 - 1, num3 + 1)]->IsCollidable;
            bool isCollidable11 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num + 1, num2 - 1, num3)]->IsCollidable;
            bool isCollidable12 = GameSingleton::blocksManager->blocks[subsystemTerrain->getCellContents(num + 1, num2 - 1, num3 + 1)]->IsCollidable;
            if ((isCollidable10 && !isCollidable11) || ((!isCollidable10 && !isCollidable11) & isCollidable12))
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, num3), Vector3(num + 1, num2 + 1, (float)(num3 + 1) - overhang.posY)));
            }
            if ((!isCollidable10 && isCollidable11) || ((!isCollidable10 && !isCollidable11) & isCollidable12))
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, num3), Vector3((float)(num + 1) - overhang.posX, num2 + 1, num3 + 1)));
            }
            if (isCollidable10 && isCollidable11)
            {
                result.emplace_back(
                    0,
                    FastVector3::Zero,
                    nullptr,
                    BoundingBox( Vector3(num, num2, num3), Vector3((float)(num + 1) - overhang.posX, num2 + 1, (float)(num3 + 1) - overhang.posY)));
            }
        }
    }

    bool ComponentBody::isColliding(const BoundingBox &box, std::vector<CollisionBox> &collisionBoxes)
    {
        for (int32_t index = 0; index < collisionBoxes.size(); index++)
        {
            if (box.intersection(collisionBoxes[index].box))
            {
                return true;
            }
        }
        return false;
    }

    bool ComponentBody::isColliding(const BoundingBox &box, std::vector<CollisionBox> &collisionBoxes1, std::vector<CollisionBox> &collisionBoxes2, std::vector<CollisionBox> &collisionBoxes3)
    {
        for(const CollisionBox& collisionBox : collisionBoxes1)
        {
            if (box.intersection(collisionBox.box))
            {
                return true;
            }
        }
        for(const CollisionBox& collisionBox : collisionBoxes2)
        {
            if (box.intersection(collisionBox.box))
            {
                return true;
            }
        }
        for(const CollisionBox& collisionBox : collisionBoxes3)
        {
            if (box.intersection(collisionBox.box))
            {
                return true;
            }
        }
        return false;
    }

    float ComponentBody::calculatePushBack(const BoundingBox &box, int32_t axis, std::vector<CollisionBox> &collisionBoxes, CollisionBox &pushingCollisionBox)
    {
        float num = 0.0f;
        for (int32_t index = 0; index < collisionBoxes.size(); index++)
        {
            float num2 = calculateBoxBoxOverlap(box, collisionBoxes[index].box, axis);
            if (std::abs(num2) > std::abs(num))
            {
                num = num2;
                pushingCollisionBox = collisionBoxes[index];
            }
        }
        return num;
    }

    float ComponentBody::calculateSmoothRisePushBack(
        const BoundingBox &normalBox,
        const BoundingBox &smoothRiseBox,
        int32_t axis,
        std::vector<CollisionBox> &collisionBoxes,
        CollisionBox &pushingCollisionBox)
    {
        float num = 0.0f;
        for (int32_t index = 0; index < collisionBoxes.size(); index++)
        {
            float num2;
            if (false == GameSingleton::blocksManager->blocks[Terrain::ExtractContents(collisionBoxes[index].blockValue)]->NoSmoothRise)
            {
                num2 = calculateBoxBoxOverlap(smoothRiseBox, collisionBoxes[index].box, axis);
            }
            else
            {
                num2 = calculateBoxBoxOverlap(normalBox, collisionBoxes[index].box, axis);
            }
            if (std::abs(num2) > std::abs(num))
            {
                num = num2;
                pushingCollisionBox = collisionBoxes[index];
            }
        }
        return num;
    }
    
    const Matrix &ComponentBody::getMatrix()
    {
        std::lock_guard<std::mutex> lk(m_matrixMutex);
        if (m_needUpdateMatrix == true)
        {
            m_needUpdateMatrix = false;

            m_rotationMutex.lock();
            m_matrix = m_rotation.toMatrix();
            m_rotationMutex.unlock();

            m_positionMutex.lock();
            m_matrix.setTranslation(m_position);
            m_positionMutex.unlock();
        }
        return m_matrix;
    }

    static const std::string componentName = "Body";
    const std::string &ComponentBody::getName() const
    {
        return componentName;
    }
    const std::string &ComponentBody::GetName()
    {
        return componentName;
    }
    void ComponentBody::load(const nlohmann::json &jsonData)
    {   
        auto componentLocomotionPtr = GameSingleton::gameRegistry->try_get<ComponentLocomotion *>(entity);
        if (componentLocomotionPtr != nullptr)
        {
            componentLocomotion = *componentLocomotionPtr;
        }
        auto componentPlayerPtr = GameSingleton::gameRegistry->try_get<ComponentPlayer *>(entity);
        if (componentPlayerPtr != nullptr)
        {
            componentPlayer = *componentPlayerPtr;
        }
        auto componentCreaturePtr = GameSingleton::gameRegistry->try_get<ComponentCreature *>(entity);
        if (componentCreaturePtr != nullptr)
        {
            componentCreature = *componentCreaturePtr;
        }

        boxSize = jsonData["BoxSize"].get<Vector3>();
        mass = jsonData["Mass"].get<float>();
        density = jsonData["Density"].get<float>();
        defaultDensity = density;
        maxSmoothRiseHeight = jsonData["MaxSmoothRiseHeight"].get<float>();

        m_position = jsonData["Position"].get<Vector3>();
        m_rotation = jsonData["Rotation"].get<Quaternion>();
        m_velocity = jsonData["Velocity"].get<Vector3>();

        // 设置到出生点的位置
        if(m_position == Point3(0,-1,0))
        {
            m_position = Vector3{ GameSingleton::serverConfigure->startSpawnPoint };
            m_position.posX += GameSingleton::gameRandom->getFloat(-(GameSingleton::serverConfigure->startSpawnPointRadius), GameSingleton::serverConfigure->startSpawnPointRadius);
            m_position.posZ += GameSingleton::gameRandom->getFloat(-(GameSingleton::serverConfigure->startSpawnPointRadius), GameSingleton::serverConfigure->startSpawnPointRadius);
        }
        waterDrag = jsonData["WaterDrag"].get<Vector2>();

        isGravityEnabled = true;
        isGroundDragEnabled = true;
        isWaterDragEnabled = true;
        isSmoothRiseEnabled = true;
    }
    void ComponentBody::save(nlohmann::json &jsonData)
    {
        jsonData["Position"] = m_position;
        jsonData["Rotation"] = m_rotation;
        jsonData["Velocity"] = m_velocity;
    }
    void ComponentBody::saveToMsgJson(nlohmann::json &jsonData)
    {
        MSG_VALUE_SET(jsonData, "Position", "Vector3", m_position.toString());
        MSG_VALUE_SET(jsonData, "Rotation", "Quaternion", m_rotation.toString());
        MSG_VALUE_SET(jsonData, "Velocity", "Vector3", m_velocity.toString());
    }
} // namespace PocketSurvival
